package impl

import (
	"encoding/json"
	"iron_man_comm/src/goroslib"
	"iron_man_comm/src/msg/common_msg"
	"iron_man_comm/src/tools/log"
)

type CheckImu struct {
	SensorCheckData
}

func (sensor *CheckImu) CheckData() error {
	var err error
	sensor.subscriber, err = goroslib.NewSubscriber(goroslib.SubscriberConf{
		Node:      sensor.node,
		Topic:     "/serial_com_driver/imu",
		Callback:  sensor.onCallback,
		QueueSize: 10,
	})
	if nil != err {
		return err
	}
	log.Info.Println("subscribed to [/serial_com_driver/imu]")
	sensor.EnableTimer(true)
	return nil
}
func (sensor *CheckImu) onCallback(msg *common_msg.Imu) {
	sensor.calcTime()

	if ModeData == sensor.mode { //需要打印
		bytes, err := json.Marshal(*msg)
		if nil != err {
			log.Error.Println("imu msg to json fail, err: ", err.Error())
			return
		}
		log.Info.Println("-------------------------------------------------------------")
		log.Info.Printf("%s", string(bytes))
	}
}
